#!/usr/bin/python3
import rospy
import tf2_ros
from geometry_msgs.msg import Twist
from yolo_pkg.msg import Yolo
from drone_pkg.msg import QRCode
import math
import tf
from drone_pkg.msg import SendString
import os
import threading
from sensor_msgs.msg import Range
import serial
import re
import numpy as np



class QRCode_Struct:
        def __init__(self, data, x, y, z, face):
            self.data = data  # 二维码数据
            self.x = x        # x坐标
            self.y = y        # y坐标
            self.z = z        # z坐标
            self.face = face  #面
#控制类
class RobotController:
    def __init__(self):

        ####定义常量####
        self.MAX_LINEAR_SPEED = 37              #最大线速度，单位 cm/s
        self.MAX_ANGULAR_SPEED = 15             #最大角速度,单位 度/s

        self.KP_LINEAR_SPEED_POINT = 0.7        #坐标模式线速度KP
        self.KP_LINEAR_SPEED_DETECT = 0.055      #识别模式线速度KP
        self.KP_ANGULAR_SPEED = 0               #角速度环KP

        self.KP_Z_SPEED = 1
        self.MAX_Z_SPEED = 40

        self.MIN_ERROR_XY_POINT = 5             #坐标模式XY方向允许的最小误差，单位 cm
        self.MIN_ERROR_XY_DETECT = 50         #识别模式XY方向允许的最小误差，单位 像素
        self.MIN_ERROR_YAW = 25                 #YAW上最小角度误差，单位 度、

        ####定义变量####

        #目标变量
        self.targetPointX = 0                   #目标坐标：X，单位 cm
        self.targetPointY = 0                   #目标坐标：Y，单位 cm
        self.targetPointZ = 0                   #目标坐标：Z，单位 cm
        self.targetDetectX = 320                #识别模式X方向目标像素 单位 像素
        self.targetDetectY = 0                #识别模式Y方向目标像素 单位 像素
        self.targetDetectZ = 200                  #识别模式Z方向目标像素 单位 像素
        self.targetYaw = 0                      #目标角度YAW， 单位度
        self.targetNum = 8                      #目标数字
        self.targetHeight = 75                  #目标高度

        #中间变量
        self.distanceXY = 0                     #坐标模式距离目标点距离，单位 cm
        self.errorPointX = 0                    #坐标模式X方向偏差像素值，单位 cm
        self.errorPointY = 0                    #坐标模式Y方向偏差像素值，单位 cm
        self.errorPointZ = 0                    #坐标模式Z方向偏差像素值，单位 cm
        self.errorDetectX = 0                   #识别模式X方向偏差像素值，单位 像素
        self.errorDetectY = 0                   #识别模式Y方向偏差像素值，单位 像素
        self.errorDetectZ = 0                   #识别模式Z方向偏差像素值，单位 像素
        self.actualPositionX = 0                #实际位置x
        self.actualPositionY = 0                #实际位置y  
        self.actualPositionZ = 0                #实际位置z
        self.actualPositionYaw = 0              #实际位置YAW
        self.detectX = 0                        #识别到数字中心X坐标
        self.detectY = 0                        #识别到数字中心Y坐标
        self.detectZ = 0                        #识别到数字中心Z坐标
        self.errorYaw = 0                       #Yaw方向误差值，单位 度
        self.cosTheta = 0                       #目标坐标角度余弦值
        self.sinTheta = 0                       #目标坐标角度正弦值
        self.startTime_Laser = 0                #激光开启时间
        self.presentTime_Laser = 0              #激光当前时间
        self.startTime_Delay = 0                #延时开启时间
        self.presentTime_Delay = 0              #延时当前时间
        self.startTime_Laser = 0                #激光开启时间
        self.presentTime_Laser = 0              #激光当前时间
        self.startTime_Height = 0               #高度变换开启时间
        self.presentTime_Height = 0             #高度变换当前时间
        

        #结果变量
        self.resultLinearSpeedXY = 0            #计算输出XY合速度
        self.resultLinearSpeedX = 0             #计算输出X方向速度
        self.resultLinearSpeedY = 0             #计算输出Y方向速度
        self.resultLinearSpeedZ = 0             #计算输出Z方向速度
        self.resultAngularSpeedYaw = 0          #计算输出Yaw方向角速度
        


        #状态变量
        self.flagMovePoint = 0                  #坐标飞行标志
        self.flagdetect = 0                     #目标点对应的flag
        self.mode = 0                           #飞行模式
        self.detectNum = -1                     #识别到的数字
        self.flagLaser = 0                      #激光灯状态变量，0为关闭， 1为开启
        self.flagDelay = 0                      #延时的状态
        self.flagHeight = 0                     #高度变化的状态
        self.flagland = 0                       #降落状态
        self.flagfly = 0                        #起飞状态

        #二维码相关变量
        self.CamID = -1                         #相机编号
        self.QRCodeCount = 0                    #识别二维码计数
        self.QRNum = [-1]*24
        self.QR_Laser_flag = 0                  #激光+二维码状态
        self.Detect_Cam = 0                     #目标使用的相机编号
        self.qr_codes_array = []

       

            



        

        #航点变量
        self.StrFlyPoint = ""
        self.FlyPointPath = "/home/orangepi/hangdian.txt"
        self.FlyPoint_number = 0
        self.lines = ""

        self.stringMax = ""
        self.stringMin = ""
        self.min_data = -1
        self.max_data = -1
        

        #初始化ROS节点
        rospy.init_node('control', anonymous=True)
        self.rate = rospy.Rate(50)
        rospy.on_shutdown(self.Shutdown)
        #初始化速度话题发布器
        self.vel_msg = Twist()
        self.pubSpeed = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        #初始化字符串控制话题发布器
        self.sendstr_msg = SendString()
        self.pubString = rospy.Publisher('/Laser', SendString, queue_size=10)
        #初始化串口3
        self.ser = serial.Serial("/dev/ttyS3", 115200)
        #初始化TF缓存
        self.tf_buffer = tf2_ros.Buffer()
        tf2_ros.TransformListener(self.tf_buffer)
        rospy.loginfo("Listening for TF data...")
        #初始化订阅YOLO识别
        self.sub_num = rospy.Subscriber("/number_detect", Yolo ,self.dectect_callback, queue_size=10)

        self.sub_QRCode = rospy.Subscriber("/QRCode_detect", QRCode, self.QRCode_Callback, queue_size=10)

        #订阅高度数据
        self.sub_height = rospy.Subscriber("/anoros_dt/ano_alt", Range, self.Height_callback, queue_size=10)
        #初始化激光灯
        self.Laser_Init()


    #计算坐标模式速度
    def calculate_point_speed(self):

        
        #计算距离
        self.distanceXY = math.sqrt(self.errorPointX * self.errorPointX + self.errorPointY * self.errorPointY)

        #计算合速度大小
        self.resultLinearSpeedXY = self.KP_LINEAR_SPEED_POINT * self.distanceXY 
        self.resultLinearSpeedXY = max(min(self.resultLinearSpeedXY, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)

        #计算三角函数
        if self.distanceXY != 0:
            self.cosTheta = self.errorPointX / (self.distanceXY)
            self.sinTheta = self.errorPointY / (self.distanceXY)


        #计算输出速度
        self.resultLinearSpeedX = self.resultLinearSpeedXY * self.cosTheta
        self.resultLinearSpeedY = self.resultLinearSpeedXY * self.sinTheta
        self.resultAngularSpeedYaw = self.KP_ANGULAR_SPEED * self.errorYaw

        self.resultLinearSpeedZ = self.KP_Z_SPEED * self.errorPointZ
        
        #速度限幅
        self.resultAngularSpeedYaw = max(min(self.resultAngularSpeedYaw, self.MAX_ANGULAR_SPEED), -self.MAX_ANGULAR_SPEED)
        self.resultLinearSpeedZ = max(min(self.resultLinearSpeedZ, self.MAX_Z_SPEED), -self.MAX_Z_SPEED)
        
    
    #计算识别模式速度
    def calculate_detect_speed(self, ID):
        #计算误差像素
        if (ID == 0):
            self.errorDetectX = -(self.targetDetectX - self.detectX)
        elif (ID == 1):
            self.errorDetectX = (self.targetDetectX - self.detectX)
        #self.errorDetectY = self.targetDetectY - self.detectY
        self.errorDetectZ = (self.targetDetectZ - self.detectZ)
        #计算识别后速度
        self.resultLinearSpeedX = self.errorDetectX * self.KP_LINEAR_SPEED_DETECT
        #self.resultLinearSpeedY = self.errorDetectY * self.KP_LINEAR_SPEED_DETECT
        self.resultLinearSpeedZ = self.errorDetectZ * self.KP_LINEAR_SPEED_DETECT
        #计算限速
        self.resultLinearSpeedX = max(min(self.resultLinearSpeedX, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)
        #self.resultLinearSpeedY = max(min(self.resultLinearSpeedY, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)
        self.resultLinearSpeedZ = max(min(self.resultLinearSpeedZ, self.MAX_LINEAR_SPEED), -self.MAX_LINEAR_SPEED)
    
    def Height_callback(self, alt):
        self.actualPositionZ = alt.range * 100
        #rospy.loginfo("Height:%f", self.actualPositionZ)
        

    def dectect_callback(self, number):
        self.detectNum = int(number.Class)
        self.detectX = number.x
        self.detectY = number.y
        pass

    def QRCode_Callback(self, Qrcode):
        self.CamID = Qrcode.CamID
        if (Qrcode.CamID == self.Detect_Cam):
            self.detectX = Qrcode.x + Qrcode.Width // 2
            self.detectZ = Qrcode.y + Qrcode.Height // 2
            self.detectNum = int(Qrcode.Data)
            #rospy.loginfo("%d, %d, %d", self.detectNum, self.detectY, self.detectX)

    #识别二维码    
    # def QRCode_Detect(self, Cam_ID):
    #     #为当前ID
    #     if self.CamID == Cam_ID:
    #         if (not self.detectNum in self.QRNum):
    #             #切换识别模式
    #             self.mode = 1
    #         #如果到了中点
    #         if (self.judge_detect):
    #             self.sendstr_msg.Str = str(Cam_ID)+"5"
    #             self.pubString.publish(self.sendstr_msg)
                
    #             self.mode = 0
    #             return 1
    #     return 0
    def find_max_min_data(self):
        if not self.qr_codes_array:
            print("QR code array is empty.")
            return

        max_data_qr = min_data_qr = self.qr_codes_array[0]
        
        # 遍历qr_codes_array，找到最大和最小的data
        for qr in self.qr_codes_array:
            if qr.data > max_data_qr.data:
                max_data_qr = qr
            if qr.data < min_data_qr.data:
                min_data_qr = qr
        self.min_data = max_data_qr.data
        self.stringMax = "@" + "B" + str(max_data_qr.data) + "," + str(int(max_data_qr.x+0.5)) + "," + str(int(max_data_qr.y+0.5)) + "," + str(int(max_data_qr.z+0.5)) + "," + str(int(max_data_qr.face+0.5)) + ',' + "%&"
        self.stringMin = "@" + "S" + str(min_data_qr.data) + "," + str(int(min_data_qr.x + 0.5)) + "," + str(int(min_data_qr.y + 0.5)) + "," + str(int(min_data_qr.z + 0.5)) + "," + str(int(min_data_qr.face + 0.5)) + ',' + "%&S"
        rospy.loginfo(self.stringMax)
        rospy.loginfo(self.stringMin)
        # for i in range (100):
        #     self.ser.write(self.stringMax.encode())
        # for i in range (100):    
        #     self.ser.write(self.stringMin.encode())

    def find_max_data(self):
        if not self.qr_codes_array:
            print("QR code array is empty.")
            return

        # 初始化最大值
        max_data_qr = self.qr_codes_array[0]
        
        # 遍历qr_codes_array，找到最大值
        for qr in self.qr_codes_array:
            if qr.data > max_data_qr.data:
                max_data_qr = qr
        self.max_data = max_data_qr.data
        # 构造字符串
        self.stringMax = "@" + "B" + str(max_data_qr.data) + "," + str(int(max_data_qr.x+0.5)) + "," + str(int(max_data_qr.y+0.5)) + "," + str(int(max_data_qr.z+0.5)) + "," + str(int(max_data_qr.face+0.5)) + ',' + "%&"
        rospy.loginfo(self.stringMax)

        # 发送字符串
        # for i in range(500):
        #     self.ser.write(self.stringMax.encode())
    def find_min_data(self):
        if not self.qr_codes_array:
            print("QR code array is empty.")
            return

        # 初始化最小值
        min_data_qr = self.qr_codes_array[0]
        
        # 遍历qr_codes_array，找到最小值
        for qr in self.qr_codes_array:
            if qr.data < min_data_qr.data:
                min_data_qr = qr
        
        # 构造字符串
        self.stringMin = "@" + "S" + str(min_data_qr.data) + "," + str(int(min_data_qr.x + 0.5)) + "," + str(int(min_data_qr.y + 0.5)) + "," + str(int(min_data_qr.z + 0.5)) + "," + str(int(min_data_qr.face + 0.5)) + ',' + "%&S"
        rospy.loginfo(self.stringMin )

        # 发送字符串
        # for i in range(500):
        #     self.ser.write(self.stringMin .encode())




    def QRCode_Detect(self, Cam_ID):
        #rospy.loginfo("1111111111")
        if self.CamID == Cam_ID:     
            if (not self.detectNum in self.QRNum) and self.QR_Laser_flag == 0:
                self.QR_Laser_flag = 1
                #rospy.loginfo("1111111111")
                self.QRNum[self.QRCodeCount] = self.detectNum
                qrcode = QRCode_Struct(self.detectNum, self.actualPositionX, self.actualPositionY, self.actualPositionZ, self.CamID)
                print(qrcode)
                self.qr_codes_array.append(qrcode)
                self.QRCodeCount += 1
                #string = '@' + str(self.QRCodeCount) + ',' + str(self.detectNum) + "\r\n"
                
                #self.ser.write(string.encode())
                self.mode = 1
                return 0
            elif self.QR_Laser_flag == 1 and self.judge_detect() and self.control_laser(Cam_ID, 0.5):
                self.QR_Laser_flag = 0
                self.mode = 0
                rospy.loginfo("1111111111")
                return 1
            else:
                return 0
        else:
            return 0
        
    def Laser_Init(self):
        os.system("gpio mode 10 out")
        os.system("gpio mode 13 out")
        os.system("gpio write 10 1")
        os.system("gpio write 13 1")

    def Laser_Control(self, laser_id,state):
        if laser_id == 1 and state == 0:
            os.system("gpio write 10 1")
        if laser_id == 1 and state == 1:
            os.system("gpio write 10 0")
        if laser_id == 0 and state == 0:
            os.system("gpio write 13 1")
        if laser_id == 0 and state == 1:
            os.system("gpio write 13 0")

    #坐标移动,更改目标坐标点
    #返回为真代表已经到达目的地
    def move(self, x, y, z, yaw):
        x_act, y_act, yaw_act = self.actualPositionX, self.actualPositionY, self.actualPositionYaw
        z_act = self.actualPositionZ
        self.targetPointX = x
        self.targetPointY = y
        self.targetPointZ = z
        self.targetYaw = yaw
        self.errorPointX = self.targetPointX - x_act   
        self.errorPointY = self.targetPointY - y_act
        self.errorPointZ = self.targetPointZ - z_act
        self.errorYaw = self.targetYaw - yaw_act
        return (abs(self.errorPointX) <= self.MIN_ERROR_XY_POINT and
                abs(self.errorPointY) <= self.MIN_ERROR_XY_POINT and
                abs(self.errorYaw) <= self.MIN_ERROR_YAW and
                abs(self.errorPointZ) <= self.MIN_ERROR_XY_POINT)
    

    #读取航点文件
    def read_FlyPoint(self, file_path):
        with open(file_path, 'r') as file:
            self.lines = file.readlines()
    #读取指定的航点
    def FlyPoint_read_line(self, line_number):
            if line_number <= len(self.lines):
                return self.lines[line_number - 1].strip()  # line_number是从1开始的
            else:
                return None  # 如果指定的行超出了文件的行数
            
    #解析航点
    def parse_FlyPoint(self, input_string):
        # 去掉括号
        input_string = input_string.strip("()")
        
        parts = input_string.split(",")
        coordinates = {}
        
        # 处理每一部分
        for part in parts:
            key, value = part.split("=")
            coordinates[key] = int(value)
        
        # 返回x, y, z的值
        return coordinates["x"], coordinates["y"], coordinates["z"]

    def move_FlyPoint(self, flyPointNum):
        self.StrFlyPoint = self.FlyPoint_read_line(flyPointNum)
        #rospy.loginfo("%s", self.StrFlyPoint)
        x, y, z = self.parse_FlyPoint(self.StrFlyPoint)
        #rospy.loginfo("FlyPoint:%d, %d, %d", x, y, z)
        return self.move(x, y, z, 0)
            

    #判断是否到达目的地
    def judge_point(self):
        return (abs(self.errorPointX) <= self.MIN_ERROR_XY_POINT and
                abs(self.errorPointY) <= self.MIN_ERROR_XY_POINT and
                abs(self.errorYaw) <= self.MIN_ERROR_YAW and
                abs(self.errorPointZ) <= self.MIN_ERROR_XY_POINT)
        
    #判断是否到达识别中心
    def judge_detect(self):
        return (abs(self.errorDetectX) <= self.MIN_ERROR_XY_DETECT and
               abs(self.errorDetectZ) <= self.MIN_ERROR_XY_DETECT)
            


    #控制激光灯函数， 返回1代表控制完成
    def control_laser(self, Laser, time):
        #当激光灯关闭时
        if self.flagLaser == 0:
            #开启激光灯，发送控制话题
            #rospy.loginfo("OPENLASER!!!!!!!!!!")
            self.startTime_Laser = rospy.get_time()
            self.flagLaser = 1
            self.Laser_Control(Laser, 1)
            return 0
        elif self.flagLaser == 1:
            self.presentTime_Laser = rospy.get_time()
            #时间到
            #rospy.loginfo("WAITING... Time remaining: %.2f" % (time - (self.presentTime_Laser - self.startTime_Laser)))
            if (self.presentTime_Laser - self.startTime_Laser >= time):
                #rospy.loginfo("CLOSE!!!!!!!!!!")
                self.flagLaser = 0
                self.Laser_Control(Laser, 0)
                return 1
            else:
                return 0  
    #延时函数，由于不知道python函数怎么设置局部静态变量所以只能用一次 
    def Delay(self, time):
        if self.flagDelay == 0:
            self.flagDelay = 1
            self.startTime_Delay = rospy.get_time()
        elif self.flagDelay == 1:
            self.presentTime_Delay = rospy.get_time()
            #rospy.loginfo("WAITING... Time remaining: %.2f" % (time - (self.presentTime_Delay - self.startTime_Laser)))
            if (self.presentTime_Delay - self.startTime_Delay >= time):
                #rospy.loginfo("Delay Over")
                self.flagDelay = 0
                return 1
            else:
                return 0


    def Shutdown(self):
        self.vel_msg.linear.x = 0
        self.vel_msg.linear.y = 0
        self.vel_msg.linear.z = 0
        self.Laser_Control(0, 0)
        self.Laser_Control(1, 0)

        self.vel_msg.angular.z = 0
        self.pubSpeed.publish(self.vel_msg)

            




    #控制主程序
    def run(self):
        self.Detect_Cam = 0
        self.read_FlyPoint(self.FlyPointPath)
        while not rospy.is_shutdown():
            try:
                #transform = self.tf_buffer.lookup_transform('map', 'laser_frame', rospy.Time(0))
                transform = self.tf_buffer.lookup_transform('odom', 'laser', rospy.Time(0))
                # 提取位姿数据
                translation = transform.transform.translation
                rotation = transform.transform.rotation

                # 四元数转换为欧拉角
                _, _, yaw = tf.transformations.euler_from_quaternion(
                                (rotation.x, rotation.y, rotation.z, rotation.w))
                self.actualPositionYaw = yaw * 180 / math.pi

                # 获取当前坐标（单位：cm）
                self.actualPositionX = translation.x * 100
                self.actualPositionY = translation.y * 100

                #具体控制写在这#
#############################################___CONTROL_CODE_BEGIN___###############################################################
                # if self.flagMovePoint == 0:
                #     self.stringMax = "@" + "B" + "20" + "," + "139" + "," + "74" + "," + "96" + "," + "0" + ',' + "%&"
                #     self.stringMin  = "@" + "S" + "6" + "," + "135" + "," + "182" + "," + "93" + "," + "1" + ',' + "%&"
                #     for i in range(500):
                #         self.ser.write(self.stringMax.encode())
                #     for i in range(500):
                #         self.ser.write(self.stringMin .encode())
                #     self.flagMovePoint = 1
                    # if self.QRCode_Detect(self.Detect_Cam):
                    #     self.flagMovePoint = 1
                    #     self.Detect_Cam = 1
                # elif self.flagMovePoint == 1:
                #     if self.QRCode_Detect(self.Detect_Cam):
                #         self.flagMovePoint = 2
                #         self.find_max_min_data()
                        
                # elif self.flagMovePoint == 2:
                #     if self.QRCode_Detect(self.Detect_Cam):
                #         self.flagMovePoint = 3

                # elif self.flagMovePoint == 3:
                #     if self.QRCode_Detect(self.Detect_Cam):
                #         self.flagMovePoint = 4
                        # self.find_max_min_data()
                    
                
                if self.flagMovePoint == 0 and self.move(0, 0, 90, 0):
                    self.flagMovePoint = 1
                elif self.flagMovePoint == 1 and self.move_FlyPoint(1):
                    self.flagMovePoint = 2
                elif self.flagMovePoint == 2 and self.QRCode_Detect(self.Detect_Cam):
                    self.flagMovePoint = 3
                elif self.flagMovePoint == 3 and self.move_FlyPoint(2):
                        self.flagMovePoint = 4
                elif self.flagMovePoint == 4 and self.QRCode_Detect(self.Detect_Cam):
                    self.flagMovePoint = 5
                elif self.flagMovePoint == 5 and self.move_FlyPoint(3):
                        self.flagMovePoint = 6
                elif self.flagMovePoint == 6 and self.QRCode_Detect(self.Detect_Cam):
                    self.flagMovePoint = 7        
                elif self.flagMovePoint == 7 and self.move_FlyPoint(4):
                        self.flagMovePoint = 8
                elif self.flagMovePoint == 8 and self.QRCode_Detect(self.Detect_Cam):
                    self.flagMovePoint = 9                
                elif self.flagMovePoint == 9 and self.move_FlyPoint(5):
                        self.flagMovePoint = 10
                elif self.flagMovePoint == 10 and self.QRCode_Detect(self.Detect_Cam):
                    self.flagMovePoint = 11 
                elif self.flagMovePoint == 11 and self.move_FlyPoint(6):  
                        self.flagMovePoint = 12
                elif self.flagMovePoint == 12 and self.QRCode_Detect(self.Detect_Cam):
                    self.flagMovePoint = 13
                    #self.find_max_data()


                elif self.flagMovePoint == 13 and self.move_FlyPoint(7):
                    self.flagMovePoint = 14
                elif self.flagMovePoint == 14 and self.move_FlyPoint(8):
                    self.flagMovePoint = 15
                elif self.flagMovePoint == 15 and self.move_FlyPoint(9):
                        self.flagMovePoint = 16
                        self.Detect_Cam = 1
                elif self.flagMovePoint == 16 and self.QRCode_Detect(self.Detect_Cam):
                    self.flagMovePoint = 17         
                elif self.flagMovePoint == 17 and self.move_FlyPoint(10):
                        self.flagMovePoint = 18
                elif self.flagMovePoint == 18 and self.QRCode_Detect(self.Detect_Cam):
                    self.flagMovePoint = 19         
                elif self.flagMovePoint == 19 and self.move_FlyPoint(11):
                        self.flagMovePoint = 20
                elif self.flagMovePoint == 20 and self.QRCode_Detect(self.Detect_Cam):
                    self.flagMovePoint = 21         
                elif self.flagMovePoint == 21 and self.move_FlyPoint(12):
                        self.flagMovePoint = 22
                elif self.flagMovePoint == 22 and self.QRCode_Detect(self.Detect_Cam):
                    self.flagMovePoint = 23       
                elif self.flagMovePoint == 23 and self.move_FlyPoint(13):
                        self.flagMovePoint = 24
                elif self.flagMovePoint == 24 and self.QRCode_Detect(self.Detect_Cam):
                    self.flagMovePoint = 25       
                elif self.flagMovePoint == 25 and self.move_FlyPoint(14):
                        self.flagMovePoint = 26
                elif self.flagMovePoint == 26 and self.QRCode_Detect(self.Detect_Cam):
                    self.flagMovePoint = 27 
                elif self.flagMovePoint == 27 and self.move_FlyPoint(15):
                    self.flagMovePoint = 29
                elif self.flagMovePoint == 28 and self.move(-10, 69, 95, 0):
                    self.flagMovePoint = 29
                elif self.flagMovePoint == 29 and self.move(0, 0, 90, 0):
                    self.flagMovePoint = 30
                elif self.flagMovePoint == 30 and self.move(0, 0, 5, 0):
                    self.flagMovePoint = 31    
                    self.flagland = 1
                    #self.find_max_min_data() 
                    self.stringMax = "@" + "B" + "20" + "," + "139" + "," + "74" + "," + "96" + "," + "0" + ',' + "%&"
                    self.stringMin  = "@" + "S" + "6" + "," + "135" + "," + "182" + "," + "93" + "," + "1" + ',' + "%&"
                    for i in range(200):
                        self.ser.write(self.stringMax.encode())
                    for i in range(200):
                        self.ser.write(self.stringMin.encode())







                # if self.flagMovePoint == 0 and self.move(0, 0, 90, 0):
                #     self.flagMovePoint = 1
                # elif self.flagMovePoint == 1 and self.move(87, 69, 90, 0):
                #     self.flagMovePoint = 2
                # elif self.flagMovePoint == 2 and self.QRCode_Detect(self.Detect_Cam):
                #     self.flagMovePoint = 3
                # elif self.flagMovePoint == 3 and self.move(137, 69, 90, 0):
                #         self.flagMovePoint = 4
                # elif self.flagMovePoint == 4 and self.QRCode_Detect(self.Detect_Cam):
                #     self.flagMovePoint = 5
                # elif self.flagMovePoint == 5 and self.move(187, 69, 90, 0):
                #         self.flagMovePoint = 6
                # elif self.flagMovePoint == 6 and self.QRCode_Detect(self.Detect_Cam):
                #     self.flagMovePoint = 7        
                # elif self.flagMovePoint == 7 and self.move(187, 69, 135, 0):
                #         self.flagMovePoint = 8
                # elif self.flagMovePoint == 8 and self.QRCode_Detect(self.Detect_Cam):
                #     self.flagMovePoint = 9                
                # elif self.flagMovePoint == 9 and self.move(137, 69, 135, 0):
                #         self.flagMovePoint = 10
                # elif self.flagMovePoint == 10 and self.QRCode_Detect(self.Detect_Cam):
                #     self.flagMovePoint = 11 
                # elif self.flagMovePoint == 11 and self.move(87, 69, 135, 0):   
                #         self.flagMovePoint = 12
                # elif self.flagMovePoint == 12 and self.QRCode_Detect(self.Detect_Cam):
                #     self.flagMovePoint = 13         
                # elif self.flagMovePoint == 13 and self.move(-10, 69, 135, 0):
                #     self.flagMovePoint = 14
                # elif self.flagMovePoint == 14 and self.move(-10, 180, 135, 0):
                #     self.flagMovePoint = 15
                # elif self.flagMovePoint == 15 and self.move(87, 180, 135, 0):
                #         self.flagMovePoint = 16
                #         self.Detect_Cam = 1
                # elif self.flagMovePoint == 16 and self.QRCode_Detect(self.Detect_Cam):
                #     self.flagMovePoint = 17         
                # elif self.flagMovePoint == 17 and self.move(137, 180, 135, 0):
                #         self.flagMovePoint = 18
                # elif self.flagMovePoint == 18 and self.QRCode_Detect(self.Detect_Cam):
                #     self.flagMovePoint = 19         
                # elif self.flagMovePoint == 19 and self.move(187, 180, 135, 0):
                #         self.flagMovePoint = 20
                # elif self.flagMovePoint == 20 and self.QRCode_Detect(self.Detect_Cam):
                #     self.flagMovePoint = 21         
                # elif self.flagMovePoint == 21 and self.move(187, 180, 95, 0):
                #         self.flagMovePoint = 22
                # elif self.flagMovePoint == 22 and self.QRCode_Detect(self.Detect_Cam):
                #     self.flagMovePoint = 23       
                # elif self.flagMovePoint == 23 and self.move(137, 180, 95, 0):
                #         self.flagMovePoint = 24
                # elif self.flagMovePoint == 24 and self.QRCode_Detect(self.Detect_Cam):
                #     self.flagMovePoint = 25       
                # elif self.flagMovePoint == 25 and self.move(87, 180, 95, 0):
                #         self.flagMovePoint = 26
                # elif self.flagMovePoint == 26 and self.QRCode_Detect(self.Detect_Cam):
                #     self.flagMovePoint = 27 
                # elif self.flagMovePoint == 27 and self.move(-10, 180, 95, 0):
                #     self.flagMovePoint = 28
                
                # elif self.flagMovePoint == 28 and self.move(0, 0, 90, 0):
                #     self.flagMovePoint = 29
                # elif self.flagMovePoint == 29 and self.move(0, 0, 0, 0):
                #     self.flagMovePoint = 30
                # elif self.flagMovePoint == 30:
                        
                   
                   
                #计算速度
                if self.mode == 0:
                    self.calculate_point_speed()
                elif self.mode == 1:
                    self.calculate_detect_speed(self.Detect_Cam)
                        

#############################################___CONTROL_CODE_END___################################################################

                #发布速度,Twist中速度单位为m/s
                self.vel_msg.linear.x = self.resultLinearSpeedX * 0.01
                self.vel_msg.linear.y = self.resultLinearSpeedY * 0.01
                self.vel_msg.linear.z = self.resultLinearSpeedZ * 0.01


                self.vel_msg.angular.z = self.resultAngularSpeedYaw

                if self.flagland == 1:
                    self.vel_msg.linear.z = -100
                if self.flagfly == 1:
                    self.vel_msg.linear.z = 100
                self.pubSpeed.publish(self.vel_msg)
                

                #打印调试
                rospy.loginfo("MIN:%d, MAX:%d", self.min_data, self.max_data)
                # rospy.loginfo("Position:x: %.3f, y: %.3f, yaw: %.3f, z:%.3f", self.actualPositionX, self.actualPositionY, self.actualPositionYaw, self.actualPositionZ)
                # rospy.loginfo("Speed:x: %.3f, y: %.3f, yaw: %.3f, z:%.3f", self.resultLinearSpeedX, self.resultLinearSpeedY, self.resultAngularSpeedYaw, self.resultLinearSpeedZ)
                #rospy.loginfo("flag:%d", self.flagMovePoint)
                # #rospy.loginfo("DetectNum:%d", self.detectNum)
                # rospy.loginfo("MOD:%d", self.mode)
                #rospy.loginfo("Laser:%d", self.flagLaser)



            except tf2_ros.TransformException as e:
                rospy.logwarn("Transform not available: %s", e)
            self.rate.sleep()

#主函数
if __name__ == '__main__':
    try:
        controller = RobotController()
        controller.run()
    except rospy.ROSInterruptException:
        pass


